/*
frigOS
Licensed under GPLv3
*/
#include "timer.h"
#include "kernel.h"
#include "led.h"

#include "serial.h"

#include <avr/interrupt.h>
#include <avr/io.h>

#include <util/delay.h>

volatile uint16_t ticks1;
volatile uint16_t ticks3;

// monitor thread that has been scheduled to run
volatile MCB* scheduledMonitor = NULL;
volatile BOOL continueAsynchLoop = TRUE;

// set a watchdog that will reset the board if it ever hangs up
void initWatchdog(void)
{
    asm volatile("\t wdr\n"::);
    WDTCR = (1 << WDCE) | (1 << WDE);
    WDTCR = (1 << WDE) | (1 << WDP2) | (1 << WDP1) | (1 << WDP0);
}

// initialize timer 1 to fire an interrupt every 1ms
// used to trigger context switches between threads
void initTimer1(void)
{

    TCCR1A = 0;
    // 16MHz/256
    //TCCR1B = (1 << CS12);
    // 16MHz/64
    TCCR1B = (1 << CS11) | (1 << CS10);
    TIFR |= (1 << TOV1);
    // enable Timer1 overflow interrupt
    TIMSK |= (1 << TOIE1);

} // initTimer1

// initialize timer 2
// TODO: this is incomplete, and should not be used yet
void initTimer2(void)
{
    TCCR2 = 0;

    // enable Timer2 overflow interrupt
    TIMSK |= (1 << TOIE2);
}

// initialize timer 3 to produce system ticks at 10/sec
void initTimer3(void)
{

    TCCR3A = 0;
    // 16MHz/256
    TCCR3B = (1 << CS32);
    TCCR3C = 0;
    ETIFR |= (1 << TOV3);
    // enable Timer3 overflow interrupt
    ETIMSK |= (1 << TOIE3);

} // initTimer3

// Task Context Switch
// push the current processor state onto the stack, schedule
// the next task, execute a monitor thread (if necessary),
// and pop the new task's state off the stack
ISR(TIMER1_OVF_vect, ISR_NAKED ISR_BLOCK)
{
    // save state
    // general and special purpose registers
    asm volatile(
        "\t push __zero_reg__\n"
        "\t push __tmp_reg__\n"
        "\t in __tmp_reg__,__SREG__\n"
        "\t push __tmp_reg__\n"
        "\t in __tmp_reg__,0x3b\n"
        "\t push __tmp_reg__\n"
        "\t clr  __zero_reg__\n"
        "\t push r2\n"
        "\t push r3\n"
        "\t push r4\n"
        "\t push r5\n"
        "\t push r6\n"
        "\t push r7\n"
        "\t push r8\n"
        "\t push r9\n"
        "\t push r10\n"
        "\t push r11\n"
        "\t push r12\n"
        "\t push r13\n"
        "\t push r14\n"
        "\t push r15\n"
        "\t push r16\n"
        "\t push r17\n"
        "\t push r18\n"
        "\t push r19\n"
        "\t push r20\n"
        "\t push r21\n"
        "\t push r22\n"
        "\t push r23\n"
        "\t push r24\n"
        "\t push r25\n"
        "\t push r26\n"
        "\t push r27\n"
        "\t push r28\n"
        "\t push r29\n"
        "\t push r30\n"
        "\t push r31\n"
    :
    :
    );


    //  changeLED( AUX_LED, LED_TOGGLE );
    asm volatile("\t wdr\n"::);
    if ( currentThread != MAXIMUM_TASKS )
    {
        thread[currentThread].stackPtr = (void *) SP;
    }

    /*// 1 shot 10ms
    // 16MHz / 256 * 1/100 = 625
    // 2^16 - 625 = 64911
    TCNT1 = 64911;*/

    // 1 shot 1ms
    // 16MHz / 64 * 1/1000 = 250
    // 2^16 - 250 = 65286
    TCNT1 = 65286;


    ticks1++;

    // enable interrupts so we don't lose bytes on the serial ports
    sei();

    // schedule the next thread
    schedule();

    // schedule a monitor thread if necessary
    //sei();
    scheduledMonitor = scheduleMonitorThreads();
    if(scheduledMonitor!=NULL && scheduledMonitor->jmpBuf==NULL)
    {
        scheduledMonitor->execute();
        continueAsynchLoop = TRUE;
    }
    else if(scheduledMonitor!=NULL && scheduledMonitor->jmpBuf!=NULL)
    {
        continueAsynchLoop = scheduledMonitor -> execute();
    }
    else
    {
        continueAsynchLoop = TRUE;
    }

    // disable interrupts again while we pop everything off the stack
    cli();

    // restore the state of the scheduled task

    if ( currentThread != MAXIMUM_TASKS )
    {
        SP = (uint16_t) thread[currentThread].stackPtr;
    }

    // general and special purpose registers
    asm volatile(
        "\t pop r31\n"
        "\t pop r30\n"
        "\t pop r29\n"
        "\t pop r28\n"
        "\t pop r27\n"
        "\t pop r26\n"
        "\t pop r25\n"
        "\t pop r24\n"
        "\t pop r23\n"
        "\t pop r22\n"
        "\t pop r21\n"
        "\t pop r20\n"
        "\t pop r19\n"
        "\t pop r18\n"
        "\t pop r17\n"
        "\t pop r16\n"
        "\t pop r15\n"
        "\t pop r14\n"
        "\t pop r13\n"
        "\t pop r12\n"
        "\t pop r11\n"
        "\t pop r10\n"
        "\t pop r9\n"
        "\t pop r8\n"
        "\t pop r7\n"
        "\t pop r6\n"
        "\t pop r5\n"
        "\t pop r4\n"
        "\t pop r3\n"
        "\t pop r2\n"
        "\t pop __tmp_reg__\n"
        "\t out 0x3b,__tmp_reg__\n"
        "\t pop __tmp_reg__\n"
        "\t out __SREG__,__tmp_reg__\n"
        "\t pop __tmp_reg__\n"
        "\t pop __zero_reg__\n"
    :
    :
    );

    if(!continueAsynchLoop)
    {
        //auxLEDOn();

        // restore the monitor thread's parent thread
        // re-enable interrupts
        // execute a long-jump back into the parent thread's stack

        continueAsynchLoop = TRUE;
        stopMonitorThread(scheduledMonitor -> tid);
        thread[currentThread].state = READY;

        sei();
        longjmp(*(scheduledMonitor -> jmpBuf), 1);
    }
    else
    {
        //auxLEDOff();
        asm volatile("\t reti\n"::);
    }
}

// timer 2 overflow interrupt; simply fires the overflow event
ISR(TIMER2_OVF_vect)
{
    fireEvent(TIMER2_OVERFLOW_EVENT);
}

// System "clock"
// just maintains the tick count for use
// with the sleep command
ISR(TIMER3_OVF_vect, ISR_BLOCK)
{

    // 1 shot 1/10s
    // 16MHz / 256 * 1/10 = 6250
    // 2^16 - 6250 = 59286
    TCNT3 = 59286;

    ticks3++;
    asm volatile("\t wdr\n"::);

    rxdLEDOff();
    txdLEDOff();
    //manageLEDOff();
    //changeLED(TXD_LED,LED_TOGGLE);

    // fire the timer 3 overflow event
    fireEvent(TIMER3_OVERFLOW_EVENT);
}
